import * as THREE from 'three';

function calculateInverseKinematics(targetPosition, targetRotation, initialOffset) {
    // 初始化中间变量
    const positionVector = new THREE.Vector3();
    const eulerRotation = new THREE.Euler();
    let solution = {};

    // 处理初始偏移参数
    if (initialOffset) {
        solution = {
            pos: new THREE.Vector3(-initialOffset[2], initialOffset[0], initialOffset[1] - OFFSET_F),
            euler: new THREE.Euler(0, 0, 0, "YXZ")
        };
    } else {
        // 验证输入参数有效性
        if (!targetPosition || !targetPosition.isVector3) return;
        if (targetRotation && !targetRotation.isEuler) return;

        // 复制并转换坐标
        positionVector.copy(targetPosition);
        if (targetRotation) eulerRotation.copy(targetRotation);

        // 应用坐标变换
        positionVector.sub(new THREE.Vector3(0, OFFSET_F, 0));
        solution = {
            pos: new THREE.Vector3(
                positionVector.x,
                -positionVector.z,
                positionVector.y
            ),
            euler: new THREE.Euler(
                -eulerRotation.y - Math.PI,
                -eulerRotation.z + Math.PI / 2,
                -eulerRotation.x + Math.PI / 2,
                "YXZ"
            )
        };
    }

    // 创建旋转矩阵
    const rotationMatrix = new THREE.Matrix4();
    rotationMatrix.makeRotationFromEuler(solution.euler);

    // 提取矩阵元素（假设为3x3旋转矩阵）
    const [
        R11, R12, R13,
        R21, R22, R23,
        R31, R32, R33
    ] = [
        rotationMatrix.elements[0], rotationMatrix.elements[1], rotationMatrix.elements[2],
        rotationMatrix.elements[4], rotationMatrix.elements[5], rotationMatrix.elements[6],
        rotationMatrix.elements[8], rotationMatrix.elements[9], rotationMatrix.elements[10]
    ];

    // 提取位置坐标
    const [x, y, z] = [
        solution.pos.x,
        solution.pos.y,
        solution.pos.z
    ];

    // 初始化中间变量数组
    const thetaCandidates = [];
    const solutions = [];

    // 计算初始角度候选
    const numeratorX = x - LINK_C * R31;
    const numeratorY = y - LINK_C * R32;
    const numeratorZ = z - LINK_C * R33;

    const angleCandidates = [
        Math.atan2(numeratorY, numeratorX),
        Math.atan2(numeratorY, numeratorX),
        Math.atan2(numeratorY, numeratorX) + Math.PI,
        Math.atan2(numeratorY, numeratorX) + Math.PI
    ];

    const denominator = Math.pow(numeratorX, 2) + 
                       Math.pow(numeratorY, 2) + 
                       Math.pow(numeratorZ, 2) + 
                       Math.pow(LINK_M, 2) - 
                       Math.pow(LINK_B, 2) - 
                       Math.pow(LINK_V, 2) - 
                       Math.pow(LINK_Y, 2);

    for (let i = 0; i < 4; i++) {
        thetaCandidates[i] = (denominator - 2 * LINK_M * 
            (numeratorX * Math.cos(angleCandidates[i]) + 
             numeratorY * Math.sin(angleCandidates[i]))) / (2 * LINK_B);
    }

    // 定义角度计算函数
    function calculateAngles(thetaParam, angleParam, sign) {
        const theta = thetaParam;
        const angle = angleParam;

        // 计算关节角度
        const gamma = Math.atan2(
            theta,
            Math.sqrt(Math.pow(LINK_V, 2) + Math.pow(LINK_Y, 2) - Math.pow(theta, 2)) * Math.pow(-1, sign)
        ) - Math.atan2(LINK_V, LINK_Y);

        const numeratorL = (LINK_Y + LINK_B * Math.sin(gamma)) * 
                          (numeratorX * Math.cos(angle) + 
                           numeratorY * Math.sin(angle) - LINK_M) + 
                          numeratorZ * (LINK_V + LINK_B * Math.cos(gamma));

        const denominatorL = (LINK_V + LINK_B * Math.cos(gamma)) * 
                            (numeratorX * Math.cos(angle) + 
                             numeratorY * Math.sin(angle) - LINK_M) - 
                            numeratorZ * (LINK_Y + LINK_B * Math.sin(gamma));

        const beta = Math.atan2(numeratorL, denominatorL) - gamma;

        // 计算其他角度
        const term1 = R31 * Math.sin(beta + gamma) * Math.cos(angle) + 
                      R32 * Math.sin(angle) * Math.sin(beta + gamma) - 
                      R33 * Math.cos(beta + gamma);

        const term2 = R31 * Math.cos(beta + gamma) * Math.cos(angle) + 
                      R32 * Math.sin(angle) * Math.cos(beta + gamma) + 
                      R33 * Math.sin(beta + gamma);

        const term3 = R21 * Math.sin(angle) - R22 * Math.cos(angle);

        const alpha1 = Math.atan2(term3, term2);
        const alpha2 = Math.atan2(-term3, -term2);

        const phi1 = Math.atan2(Math.sqrt(1 - Math.pow(term1, 2)), term1);
        const phi2 = Math.atan2(-Math.sqrt(1 - Math.pow(term1, 2)), term1);

        const psi1 = Math.atan2(
            R22 * Math.sin(beta + gamma) * Math.cos(angle) + 
            R23 * Math.sin(angle) * Math.sin(beta + gamma) - 
            R21 * Math.cos(beta + gamma),
            -(
                R12 * Math.sin(beta + gamma) * Math.cos(angle) + 
                R13 * Math.sin(angle) * Math.sin(beta + gamma) - 
                R11 * Math.cos(beta + gamma)
            )
        );

        const psi2 = Math.atan2(
            -R22 * Math.sin(beta + gamma) * Math.cos(angle) - 
            R23 * Math.sin(angle) * Math.sin(beta + gamma) + 
            R21 * Math.cos(beta + gamma),
            R12 * Math.sin(beta + gamma) * Math.cos(angle) + 
            R13 * Math.sin(angle) * Math.sin(beta + gamma) - 
            R11 * Math.cos(beta + gamma)
        );

        return {
            angle: angleParam,
            beta: beta,
            gamma: gamma,
            alpha: [alpha1, alpha2],
            phi: [phi1, phi2],
            psi: [psi1, psi2]
        };
    }

    // 处理不同情况
    if ((Math.pow(LINK_V, 2) + Math.pow(LINK_Y, 2) - Math.pow(thetaCandidates[0], 2) >= 0) &&
        (Math.pow(LINK_V, 2) + Math.pow(LINK_Y, 2) - Math.pow(thetaCandidates[2], 2) >= 0)) {
        for (let i = 0; i < 4; i++) {
            const angles = calculateAngles(thetaCandidates[i], angleCandidates[i], i);
            solutions.push([angles.angle, angles.beta, angles.gamma, angles.alpha[1], angles.phi[1], angles.psi[1]]);
            solutions.push([angles.angle, angles.beta, angles.gamma, angles.alpha[0], angles.phi[0], angles.psi[0]]);
        }
    } else if ((Math.pow(LINK_V, 2) + Math.pow(LINK_Y, 2) - Math.pow(thetaCandidates[0], 2) >= 0) &&
               (Math.pow(LINK_V, 2) + Math.pow(LINK_Y, 2) - Math.pow(thetaCandidates[2], 2) < 0)) {
        for (let i = 0; i < 2; i++) {
            const angles = calculateAngles(thetaCandidates[0], angleCandidates[0], i);
            solutions.push([angles.angle, angles.beta, angles.gamma, angles.alpha[0], angles.phi[0], angles.psi[0]]);
            solutions.push([angles.angle, angles.beta, angles.gamma, angles.alpha[1], angles.phi[1], angles.psi[1]]);
        }
    } else if ((Math.pow(LINK_V, 2) + Math.pow(LINK_Y, 2) - Math.pow(thetaCandidates[0], 2) < 0) &&
               (Math.pow(LINK_V, 2) + Math.pow(LINK_Y, 2) - Math.pow(thetaCandidates[2], 2) >= 0)) {
        for (let i = 0; i < 2; i++) {
            const angles = calculateAngles(thetaCandidates[2], angleCandidates[2], i);
            solutions.push([angles.angle, angles.beta, angles.gamma, angles.alpha[0], angles.phi[0], angles.psi[0]]);
            solutions.push([angles.angle, angles.beta, angles.gamma, angles.alpha[1], angles.phi[1], angles.psi[1]]);
        }
    } else {
        return [false]; // 无解
    }

    return solutions[0]; // 返回第一个解
}
export { calculateInverseKinematics as inverseKinematics};    